#pragma once
#include "ParticleFilter.h"
#include "ActionAbout.h"
#include "PoseGrabber.h"
#include "MPointCloudMap.h"
#include "Observation.h"
#include "Pose3D.h"
#include <vector>
#include<ctime>

struct confi {

	string _sequence;
	string bin_input_path;
	string pcd_input_path;
	string pose_data_path;
	//out put 
	string pcd_output_path;
	string bin_map_path;
	string logcat_path;
	string pcd_map_path;

	//PF_option
	float noisy = 0.0;
	int doParticleShift = 0;
	int shift_particle_function = 0;
	int show_shift_particles = 0;
	int particles_num = 200;
	float init_area_xy = 1.0;
	float init_area_z = 1.0;

	int display_interval = 0;

	//3d show option
	int show_3d = 0;

	int show_ground_truth = 0;

	int show_ground_truth_with_noisy = 0;

	int color_with_z = 0;

};

class PF_LocationApplication
{
public:
	PF_LocationApplication();
	~PF_LocationApplication();
	void appInit();
	void appRun();
	void readConfig();
	static void show_prediction_particles(bool* has_add,
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr show_particles,
		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_pt) {
		if (has_add!=nullptr && show_particles != nullptr && viewer_pt != nullptr) {
			if (*has_add==false) {
				viewer_pt->addPointCloud<pcl::PointXYZRGB>(show_particles, "particles");
				viewer_pt->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "particles");
				viewer_pt->spinOnce();
			}
			else {
				viewer_pt->updatePointCloud(show_particles, "particles");
				viewer_pt->spinOnce();
			}
		}
		else {
			LogCat::c("PF_LocationApplication","show_prediction_particles has nullptr!!!!");
		}		
	};

private:
	bool show_3d = false;
	clock_t start_time, end_time;
	MPointCloudMap map;
	PoseGrabber mPoseGrabber;
	Observation mObservation;
	ActionAbout mActionAbout;
	ParticleFilter mParticleFilter;
	vector<Pose3D> ground_truth;
	Config mConfig;
	confi mconfig;


};

